import rospkg
import matplotlib.pyplot as plt
import numpy as np
import os

def main():
    # 获取当前包的路径
    rospack = rospkg.RosPack()
    package_path = rospack.get_path('multi_camera_cooperation')
    data_file = os.path.join(package_path, 'data/2024-11-23-21-00/camD_T_base_to_servogroup.txt')

    # 读取文件中的数据
    transform_data = []
    with open(data_file, 'r') as f:
        lines = f.readlines()
        for line in lines:
            row = []
            # 分割字符串并转换为浮点数
            numbers = line.strip().split()
            # 将每个数字添加到列表中
            row.extend([float(num) for num in numbers])
            transform_data.append(row)
            

    # 将数据转换为NumPy数组
    transform_array = np.array(transform_data)
    array_shape = transform_array.shape
    frame = [i for i in range(array_shape[0])]

    T_base_to_cam = transform_array[0:array_shape[0], 0:7]
    T_base_to_cam_pre = transform_array[0:array_shape[0], 7:14]
    T_base_to_cam_gt = transform_array[0:array_shape[0], 14:21]

    # 创建图形
    plt.figure()

    # 绘制线图
    plt.plot(frame, T_base_to_cam_pre[:, 0], label='pre', color='r')
    plt.plot(frame, T_base_to_cam_gt[:, 0], label='gt', color='b')

    # 添加标题和标签
    plt.xlabel('Frame')
    plt.ylabel('x')
    plt.ylim([1, 2])

    # 显示图例
    plt.legend()

    # 显示图形
    plt.show()

if __name__ == '__main__':
    main()
